#include <math.h>

#define LAND_BASE_CORE

#include "land_math.h"
#define RA (res->A)
#define MV (mul->V)
#define MA (mul->A)

static inline LandFloat
dot(LandVector* v1, LandVector* v2) {
	return (v1->x * v2->x) + (v1->y * v2->y) 
			+ (v1->z * v2->z) + (v1->w * v2->w);
}

static inline LandFloat
dot2D(LandVector2D* v1, LandVector2D* v2) {
	return (v1->x * v2->x) + (v1->y * v2->y) + (v1->w * v2->w);
}


static inline void
cross(LandVector* res, LandVector* v) {
	LandFloat x = (res->y * v->z) - (res->z * res->y);
	LandFloat y = (res->z * v->x) - (res->x * v->z);
	LandFloat z = res->x * v->y - res->y * v->x;
	res->x = x; res->y = y; res->z = z;
}

static inline LandFloat
cross2D(LandVector2D* v1, LandVector2D* v2) {
	return (v1->x * v2->y) - (v1->y * v2->x);
}

static inline void
normalize(LandVector* res) {
	LandFloat magSq = (res->x * res->x)
		+ (res->y * res->y)
		+ (res->z * res->z);
	if (magSq > 0.0f) {
		LandFloat i = 1.0f / (LandFloat)sqrt(magSq);
		res->x *= i;
		res->y *= i;
		res->z *= i;
	}
}

static inline LandFloat
length(LandVector* res) {
	return (LandFloat)sqrt((res->x * res->x)
		+ (res->y * res->y)
		+ (res->z * res->z));
}

static inline void
add(LandVector* res, LandVector* v) {
	res->x += v->x;
	res->y += v->y;
	res->z += v->z;
}


static inline void
sub(LandVector* res, LandVector* v) {
	res->x -= v->x;
	res->y -= v->y;
	res->z -= v->z;
}


LAND_BASE_API void
landMulMatrix(LandMatrix* res, LandMatrix* mul) {
	LandFloat a1, a2, a3, a4;
	LandVector RV[4] = {
		{RA.a11, RA.a12, RA.a13, RA.a14},
		{RA.a21, RA.a22, RA.a23, RA.a24},
		{RA.a31, RA.a32, RA.a33, RA.a34},
		{RA.a41, RA.a42, RA.a43, RA.a44},
	};
	a1 = dot(&RV[0], &MV[0]);
	a2 = dot(&RV[0], &MV[1]);
	a3 = dot(&RV[0], &MV[2]);
	a4 = dot(&RV[0], &MV[3]);
	RA.a11 = a1; RA.a12 = a2; RA.a13 = a3; RA.a14 = a4;
	a1 = dot(&RV[1], &MV[0]);
	a2 = dot(&RV[1], &MV[1]);
	a3 = dot(&RV[1], &MV[2]);
	a4 = dot(&RV[1], &MV[3]);
	RA.a21 = a1; RA.a22 = a2; RA.a23 = a3; RA.a24 = a4;
	a1 = dot(&RV[2], &MV[0]);
	a2 = dot(&RV[2], &MV[1]);
	a3 = dot(&RV[2], &MV[2]);
	a4 = dot(&RV[2], &MV[3]);
	RA.a31 = a1; RA.a32 = a2; RA.a33 = a3; RA.a34 = a4;
	a1 = dot(&RV[3], &MV[0]);
	a2 = dot(&RV[3], &MV[1]);
	a3 = dot(&RV[3], &MV[2]);
	a4 = dot(&RV[3], &MV[3]);
	RA.a41 = a1; RA.a42 = a2; RA.a43 = a3; RA.a44 = a4;
}

LAND_BASE_API void
landMulMatrix2D(LandMatrix2D* res, LandMatrix2D* mul) {
	LandFloat a1, a2, a3;
	LandVector2D RV[3] = {
		{RA.a11, RA.a12, RA.a13},
		{RA.a21, RA.a22, RA.a23},
		{RA.a31, RA.a32, RA.a33},
	};
	a1 = dot2D(&RV[0], &MV[0]);
	a2 = dot2D(&RV[0], &MV[1]);
	a3 = dot2D(&RV[0], &MV[2]);
	RA.a11 = a1; RA.a12 = a2; RA.a13 = a3;
	a1 = dot2D(&RV[1], &MV[0]);
	a2 = dot2D(&RV[1], &MV[1]);
	a3 = dot2D(&RV[1], &MV[2]);
	RA.a21 = a1; RA.a22 = a2; RA.a23 = a3;
	a1 = dot2D(&RV[2], &MV[0]);
	a2 = dot2D(&RV[2], &MV[1]);
	a3 = dot2D(&RV[2], &MV[2]);
	RA.a31 = a1; RA.a32 = a2; RA.a33 = a3;
}

LAND_BASE_API void
landMoveMatrix(LandMatrix* res, LandVector* v) {
#define RA (res->A)
	if (v->x != 0.0f) {
		RA.a11 += RA.a41 * v->x;
		RA.a12 += RA.a42 * v->x;
		RA.a13 += RA.a43 * v->x;
		RA.a14 += RA.a44 * v->x;
	}
	if (v->y != 0.0f) {
		RA.a21 += RA.a41 * v->y;
		RA.a22 += RA.a42 * v->y;
		RA.a23 += RA.a43 * v->y;
		RA.a24 += RA.a44 * v->y;
	}
	if (v->z != 0.0f) {
		RA.a31 += RA.a41 * v->z;
		RA.a32 += RA.a42 * v->z;
		RA.a33 += RA.a43 * v->z;
		RA.a34 += RA.a44 * v->z;
	}
}
LAND_BASE_API void
landMoveMatrix2D(LandMatrix2D* res, LandVector2D* v) {
	LandFloat a13 = RA.a11 * v->x + RA.a12 * v->y + RA.a13;
	LandFloat a23 = RA.a21 * v->x + RA.a22 * v->y + RA.a23;
	LandFloat a33 = RA.a31 * v->x + RA.a32 * v->y + RA.a33;
	RA.a13 = a13;
	RA.a23 = a23;
	RA.a33 = a33;
}

LAND_BASE_API void
landScaleMatrix(LandMatrix* res, LandVector* v) {
#define RA (res->A)
	if (v->x != 1.0f) {
		RA.a11 *= v->x;
		RA.a12 *= v->x;
		RA.a13 *= v->x;
		RA.a14 *= v->x;
	}
	if (v->y != 1.0f) {
		RA.a21 *= v->y;
		RA.a22 *= v->y;
		RA.a23 *= v->y;
		RA.a24 *= v->y;
	}
	if (v->z != 1.0f) {
		RA.a31 *= v->z;
		RA.a32 *= v->z;
		RA.a33 *= v->z;
		RA.a34 *= v->z;
	}
}
LAND_BASE_API void
landScaleMatrix2D(LandMatrix2D* res, LandVector2D* v) {
	RA.a11 *= v->x;
	RA.a12 *= v->y;
	RA.a21 *= v->x;
	RA.a22 *= v->y;
	RA.a31 *= v->x;
	RA.a32 *= v->y;
}

static void
rotateMatrixToX(LandMatrix* res, LandFloat radian) {
	LandFloat c = (LandFloat)landCos(radian);
	LandFloat s = (LandFloat)landSin(radian);

	LandFloat v21 = res->A.a21, v22 = res->A.a22, v23 = res->A.a23, v24 = res->A.a24;
	LandFloat v31 = res->A.a31, v32 = res->A.a32, v33 = res->A.a33, v34 = res->A.a34;
	res->A.a21 = v21 * c + v31 * (-s);
	res->A.a22 = v22 * c + v32 * (-s);
	res->A.a23 = v23 * c + v33 * (-s);
	res->A.a24 = v24 * c + v34 * (-s);

	res->A.a31 = v21 * s + v31 * c;
	res->A.a32 = v22 * s + v32 * c;
	res->A.a33 = v23 * s + v33 * c;
	res->A.a34 = v24 * s + v34 * c;
}

static void
rotateMatirxToY(LandMatrix* res, LandFloat radian) {
	LandFloat c = (LandFloat)landCos(radian);
	LandFloat s = (LandFloat)landSin(radian);

	LandFloat v11 = res->A.a11, v12 = res->A.a12, v13 = res->A.a13, v14 = res->A.a14;
	LandFloat v31 = res->A.a31, v32 = res->A.a32, v33 = res->A.a33, v34 = res->A.a34;
	res->A.a11 = v11 * c + v31 * s;
	res->A.a12 = v12 * c + v32 * s;
	res->A.a13 = v13 * c + v33 * s;
	res->A.a14 = v14 * c + v34 * s;
	res->A.a31 = v11 * (-s) + v31 * c;
	res->A.a32 = v12 * (-s) + v32 * c;
	res->A.a33 = v13 * (-s) + v33 * c;
	res->A.a34 = v14 * (-s) + v34 * c;
}

static void
rotateMatrixToZ(LandMatrix* res, LandFloat radian) {
	LandFloat c = (LandFloat)landCos(radian);
	LandFloat s = (LandFloat)landSin(radian);

	LandFloat v11 = res->A.a11, v12 = res->A.a12, v13 = res->A.a13, v14 = res->A.a14;
	LandFloat v21 = res->A.a21, v22 = res->A.a22, v23 = res->A.a23, v24 = res->A.a24;
	res->A.a11 = v11 * c - v21 * s;
	res->A.a12 = v12 * c - v22 * s;
	res->A.a13 = v13 * c - v23 * s;
	res->A.a14 = v14 * c - v24 * s;
	res->A.a21 = v11 * s + v21 * c;
	res->A.a22 = v12 * s + v22 * c;
	res->A.a23 = v13 * s + v23 * c;
	res->A.a24 = v14 * s + v24 * c;
}

LAND_BASE_API void
landRotateMatrix(LandMatrix* res, LandVector* v) {
	if (v->x) {
		rotateMatrixToX(res, v->x);
	}
	if (v->y) {
		rotateMatirxToY(res, v->y);
	}
	if (v->z) {
		rotateMatrixToZ(res, v->z);
	}
}

LAND_BASE_API void
landRotateMatrixV(LandMatrix* res, LandVector* v, LandFloat radian) {
	LandVector axis = *v;
	normalize(&axis);
	LandFloat c = (LandFloat)landCos(radian);
	LandFloat s = (LandFloat)landSin(radian);
	LandFloat xx = axis.x * axis.x;
	LandFloat yy = axis.y * axis.y;
	LandFloat zz = axis.z * axis.z;
	LandFloat xy = axis.x * axis.y;
	LandFloat xz = axis.x * axis.z;
	LandFloat yz = axis.y * axis.z;
	LandMatrix r;
	landIdentityMatrix(&r);
	r.A.a11 = c + xx * (1 - c);
	r.A.a12 = xy * (1 - c) - axis.z * s;
	r.A.a13 = xz * (1 - c) + axis.y * s;
	r.A.a21 = xy * (1 - c) + axis.z * s;
	r.A.a22 = c + yy * (1 - c);
	r.A.a23 = yz * (1 - c) - axis.x * s;
	r.A.a31 = xz * (1 - c) - axis.y * s;
	r.A.a32 = yz * (1 - c) + axis.x * s;
	r.A.a33 = c + zz * (1 - c);
	landMulMatrix(res, &r);
}

LAND_BASE_API void
landRotateMatrix2D(LandMatrix2D* res, LandFloat radian) {
	LandFloat c = (LandFloat)landCos(radian);
	LandFloat s = (LandFloat)landSin(radian);
	LandFloat a11 = RA.a11, a12 = RA.a12,
		a21 = RA.a21, a22 = RA.a22,
		a31 = RA.a31, a32 = RA.a32;
	RA.a11 = a11 * c + a12 * s;
	RA.a12 = -a11 * s + a12 * c;
	RA.a21 = a21 * c + a22 * s;
	RA.a22 = -a21 * s + a22 * c;
	RA.a31 = a31 * c + a32 * s;
	RA.a32 = -a31 * s + a32 * c;
}


LAND_BASE_API void
landPerspectiveMatrix(LandMatrix* res,
	LandFloat fov, LandFloat aspect, LandFloat n, LandFloat f){
	landIdentityMatrix(res);
	//LandFloat a = (1.0f / (LandFloat)tan(fov * 0.5f));
	LandFloat a = (LandFloat)tan(fov / 2.0f);
	res->A.a11 = 1.0f / (aspect * a);
	res->A.a22 = 1.0f / a;

	//res->A.a33 = -(n + f) / (f - n);
	res->A.a33 = -(f + n) / (f - n);

	res->A.a34 = -((2 * n * f) / (f - n));
	//res->A.a34 = 1.0f;

	//res->A.a43 = -(2.0f * n * f) / (f - n);
	res->A.a43 = -1;
	//res->A.a44 = 0;

	//tan_half_angle = tan(angle / 2);
	//mat_set(to_return, 1, 1, 1 / (ratio * tan_half_angle));
	//mat_set(to_return, 2, 2, 1 / (tan_half_angle));
	//mat_set(to_return, 3, 3, -(f + n) / (f - n));
	//mat_set(to_return, 4, 3, -1);
	//mat_set(to_return, 3, 4, -(2 * f * n) / (f - n));
}

LAND_BASE_API void
landOrthoMatrix(LandMatrix* res,
	LandVector rect, LandFloat n, LandFloat f) {
	res->A.a11 = 2.0f / (rect.right - rect.left);
	res->A.a14 = -((rect.right + rect.left) / (rect.right - rect.left));
	res->A.a22 = 2.0f / (rect.top / rect.bottom);
	res->A.a24 = -((rect.top + rect.bottom) / (rect.top - rect.bottom));
	res->A.a33 = -2.0f / (f - n);
	res->A.a34 = -((f + n) / (f - n));
}

LAND_BASE_API void
landVectorMulMatrix(LandVector* res, LandMatrix* mul) {
	LandFloat x = res->x * MA.a11 + res->y * MA.a12 + res->z * MA.a13 + res->w * MA.a14;
	LandFloat y = res->x * MA.a21 + res->y * MA.a22 + res->z * MA.a23 + res->w * MA.a24;
	LandFloat z = res->x * MA.a31 + res->y * MA.a32 + res->z * MA.a33 + res->w * MA.a34;
	LandFloat w = res->x * MA.a41 + res->y * MA.a42 + res->z * MA.a43 + res->w * MA.a44;
	//LandFloat x = dot(res, &MV[0]);
	//LandFloat y = dot(res, &MV[1]);
	//LandFloat z = dot(res, &MV[2]);
	//LandFloat w = dot(res, &MV[3]);
	res->x = x; res->y = y; res->z = z; res->w = w;
}
LAND_BASE_API void
landVectorMulMatrixW(LandVector* res, LandMatrix* m, LandFloat w) {
	LandFloat x = res->x * m->A.a11 + res->y * m->A.a12 + res->z * m->A.a13 + w * m->A.a14;
	LandFloat y = res->x * m->A.a21 + res->y * m->A.a22 + res->z * m->A.a23 + w * m->A.a24;
	LandFloat z = res->x * m->A.a31 + res->y * m->A.a32 + res->z * m->A.a33 + w * m->A.a34;
	LandFloat rw = res->x * m->A.a41 + res->y * m->A.a42 + res->z * m->A.a43 + w * m->A.a44;
	res->x = x; res->y = y; res->z = z; res->w = rw;
}

LAND_BASE_API void
landVectorMulMatrix2D(LandVector2D* res, LandMatrix2D* mul) {
	LandFloat x = res->x * MA.a11 + res->y * MA.a12 + res->w * MA.a13;
	LandFloat y = res->x * MA.a21 + res->y * MA.a22 + res->w * MA.a23;
	LandFloat w = res->x * MA.a31 + res->y * MA.a32 + res->w * MA.a33;
	res->x = x; res->y = y; res->w = w;

}
LAND_BASE_API void
landVectorMulMatrix2DW(LandVector* res, LandMatrix* m, LandFloat w) {
	LandFloat x;
	x = res->x * m->A.a11 + res->y * m->A.a12 + w * m->A.a13;
	res->y = res->x * m->A.a21 + res->y * m->A.a22 + w * m->A.a23;
	res->x = x;
}

LAND_BASE_API LandFloat
landRadians(LandFloat degrees) {
	return degrees * (LAND_PI / 180.0f);
}

LAND_BASE_API LandFloat
landDegrees(LandFloat radians) {
	return radians * (180.0f / LAND_PI);
}

LAND_BASE_API void
landLookAt(LandMatrix* res, LandVector* eye, LandVector* center, LandVector* up){

	//vec<3, T, Q> const f(normalize(center - eye));
	//vec<3, T, Q> const s(normalize(cross(f, up)));
	//vec<3, T, Q> const u(cross(s, f));
	//mat<4, 4, T, Q> Result(1);
	//Result[0][0] = s.x;
	//Result[1][0] = s.y;
	//Result[2][0] = s.z;
	//Result[0][1] = u.x;
	//Result[1][1] = u.y;
	//Result[2][1] = u.z;
	//Result[0][2] = -f.x;
	//Result[1][2] = -f.y;
	//Result[2][2] = -f.z;
	//Result[3][0] = -dot(s, eye);
	//Result[3][1] = -dot(u, eye);
	//Result[3][2] = dot(f, eye);
	//return Result;

	LandVector f = *center;
	landSubVector(&f, eye);
	landNormalizeVector(&f);
	LandVector s = f;
	landCrossVector(&s, up);
	LandVector u = s;
	landCrossVector(&u, &f);

	//res->A.a11 = s.x;
	res->A.a12 = s.y;
	res->A.a13 = s.z;
	res->A.a21 = u.x;
	//res->A.a22 = u.y;
	res->A.a23 = u.z;
	res->A.a31 = -f.x;
	res->A.a32 = -f.y;
	//res->A.a33 = -f.z;
	res->A.a14 = -landDotVector(&s, eye);
	res->A.a24 = -landDotVector(&u, eye);
	res->A.a34 = landDotVector(&f, eye);


	//res->A.a31 += res->A.a41 * v->z;
	//res->A.a32 += res->A.a42 * v->z;
	//res->A.a33 += res->A.a43 * v->z;
	//res->A.a34 += res->A.a44 * v->z;
}
